#pragma once

#include <fstream>
#include <iostream>
#include <cmath>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "../include/common.hpp"
#include "./track.cpp"

#define ARC_ALLOW   20

#define START_H     ROWSIMAGE//从下到上 start较小
#define END_H       1
#define round_size  SQUARE
#define mono_num    200
#define pre_add_l   COLSIMAGE/10
#define pre_add_r   COLSIMAGE/10*9
#define ent_add     COLSIMAGE/3
#define exit_add_l  COLSIMAGE/2
#define exit_add_r  COLSIMAGE/2
#define cir_delay   100
#define cir_delay_max 400
#define cir_wait    1000
//长宽划分之后就是16;camera
#define SQUARE 16
#define FINISH_LOSE_NUM 1
#define FINISH_SATRT    (ROWSIMAGE/2)

using namespace cv;
using namespace std;

class Circle
{
private:
    long preTime;
    long startTime;
    int imageOut[2][COLSIMAGE];//0上 1下   
    enum out
    {
        up=0,
        down,
    };
    enum RingStep
    {
        None = 0, // 未知类型

        Prepareing,
        Entering, // 入环
        Inside,   // 环中
        Exiting,  // 出环
        Finish,    // 环任务结束
        Wait
    };
//方向
    enum towards
    {
        right=0,
        left
    };
//圆环类型
    enum RingType
    {   
        none_ring=0,
        right_ring,
        left_ring,
    };
    //环类型
    RingType ring_type;

    //entering阶段上一次的岔点纵坐标
    int spurroadX_last;
    //出环A点
    bool _test_A;
    int _test_UP;
    bool _left_mono;
    bool _right_mono;
    bool _right_cir;
    bool _left_cir;
//左边线    track:边线,status:朝向,num:环岛中心大小,index:环岛中心,start:寻找环岛起点,end:寻找环岛终点,down_num环岛下边限;lose丢线数量
int RoundaboutGetArc(track & track,int status, int num,int* index,int start,int end,int down_num,int lose_num)
{
    int l_up=0;
    int r_up=0;
    int l_down=0;
    int r_down=0;
    int left_flag=0;
    int right_flag=0;
    int i = 0;
    int inc = 0, dec = 0, n = 0;
    //记录丢线
    int lose=0;
    int lose_l=0;
    switch(status)
    {
    //左边线
      case 1:
          //从下到上遍历
        for(i =start-2; i >end; i--)
        {
            if(track.l_border[i].y==0&&left_flag==0&&l_down==0)
            {
                lose++;
            }
            if(track.l_border[i].y!= 0 && track.l_border[i+1].y== 0&&l_down==0)
            {
                if((track.l_border[i].y-track.l_border[i+1].y)>20){continue;}
                l_up=0;
                l_down=1;
            }
            //上下两行没有同时是边框
            if(track.l_border[i].y!= 0 && track.l_border[i+1].y!= 0&&l_down==1&&l_up==0)
            {
                left_flag=1;
                if(lose>lose_l)
                {
                    lose_l=lose;
                }
                lose=0;
                //如果两个相等
                if(track.l_border[i].y == track.l_border[i+1].y)
                {
                    n++;
                    continue;
                }
                //如果下边先小于上边线
                //下边弧
                if(track.l_border[i].y > track.l_border[i+1].y)
                {
                    //inc是下边弧的数量
                    inc++;
                    inc+=n;
                    n=0;
                }
                //如果下边线小于上边线
                //上边弧
                else if(l_down==1&&l_up==0)
                {
                    dec++;
                    dec+=n;
                    n=0;
                }

                /* 有弧线 */
                //上下弧线都大于上下边线的值
                //并且下边弧不得大于形参 down_num
                if(inc > num && dec > num && inc<down_num&&lose_l>=lose_num)
                {
                    *index = i + num;
                    l_up=1;
                    l_down=0;
                    return 1;
                }
            }
            else if (l_down==1&&l_up==0)
            {
                inc = 0;
                dec = 0;
                n=0;

            }
            if(track.l_border[i+1].y!= 0 && track.l_border[i].y== 0&&l_up==1&&l_down==0)
            {return 1;}
            // else if(l_up==1&&l_down==0){
            //     dec=0;
            //     inc=0;
            // }
        }

        break;

      case 2:
        for(i =start-2; i > end; i--)
        {
            if(track.r_border[i].y==COLSIMAGE-1&&right_flag==0&&r_down==0)
            {
                lose++;
            }
            if(track.r_border[i].y!= COLSIMAGE -1 && track.r_border[i+1].y == COLSIMAGE-1&&r_down==0)
            {
                if((track.l_border[i+1].y-track.l_border[i].y)>20){continue;}
                r_up=0;
                r_down=1;
            }
            ////////////////////////////////////////
            if(track.r_border[i].y!= COLSIMAGE -1 && track.r_border[i+1].y != COLSIMAGE-1&&r_down==1&&r_up==0)
            {
                right_flag=1;
                if(lose>lose_l)
                {
                    lose_l=lose;
                }
                lose=0;
                if(track.r_border[i].y == track.r_border[i+1].y)
                {
                    n++;
                    continue;
                }
                if(track.r_border[i].y< track.r_border[i+1].y)
                {
                    inc++;
                    inc+=n;
                    n = 0;
                }
                else if(r_down==1&&r_up==0)
                {
                    dec++;
                    dec+=n;
                    n=0;
                }

                /* 有弧线 */
                if(inc > num && dec > num && inc<down_num&&lose_l>=lose_num)
                {
                    *index = i + num;
                    r_up=1;
                    r_down=0;
                    return 1;
                }
            }
            else if(r_down==1&&r_up==0)
            {
                inc = 0;
                dec = 0;
                n=0;
            }
            if(track.r_border[i+1].y!= COLSIMAGE -1 && track.r_border[i].y == COLSIMAGE-1&&r_up==1&&r_down==0)
            {return 1;}
            // else if(r_up==1&&r_down==0){
            //     dec=0;
            //     inc=0;
            // }

        }

        break;
    }

    return 0;
}

//上下边线
int UpdownSideGet(track&track)
{

    int i = 0, j = 0;
    int last = COLSIMAGE/2;

     //从图像中间行    从中到下     从中到上      扫描
    imageOut[0][COLSIMAGE/2] =0;
    imageOut[1][COLSIMAGE/2] =ROWSIMAGE-1;

    //处理中间单独那一列的上下边线
    for(i = last; i > 0; i--)
    {
        //0,h/2
        if(!track.index[i][COLSIMAGE/2])
        {
            imageOut[0][COLSIMAGE/2] = i;
            break;
        }
    }

    for(i = last; i < ROWSIMAGE; i++)
    {
        if(!track.index[i][COLSIMAGE/2])
        {
            imageOut[1][COLSIMAGE/2] = i;
            break;
        }
    }

    //其他列的上下边线
    //从中到左
    for(i = COLSIMAGE/2-1; i > 0; i--)//遍历每一列
    {
        imageOut[0][i] = 0;
        imageOut[1][i] = ROWSIMAGE-1;

        for(j = imageOut[0][i+1] + 5; j > 0; j--)//一列中的扫描每行  从上列的行数+10开始向上扫描
        {
            //防止出界
            if(imageOut[0][i+1] + 5>ROWSIMAGE)
            {break;}

            if(!track.index[j][i])
            {
                imageOut[0][i] = j;
                break;
            }
        }
        for(j = imageOut[1][i+1] - 5; j < ROWSIMAGE; j++)
        {

            //防止出界
            if(imageOut[1][i+1] - 5<0)
            {break;}

            if(!track.index[j][i])
            {
                imageOut[1][i] = j;
                break;
            }
        }
    }

    //从中到右
    for(i = COLSIMAGE/2+1; i < COLSIMAGE-1; i++)
        {
            imageOut[0][i] = 0;
            imageOut[1][i] = ROWSIMAGE-1;

            for(j = imageOut[0][i-1] + 5; j > 0; j--)
            {
                //防出界
                if(j>ROWSIMAGE)
                {break;}

                if(!track.index[j][i])
                {
                    imageOut[0][i] = j;
                    break;
                }
            }
            for(j = imageOut[1][i-1] - 5; j < ROWSIMAGE; j++)
            {

                //防出界
                if(j- 5<0)
                {break;}

                if(!track.index[j][i])
                {
                    imageOut[1][i] = j;
                    break;
                }
            }
        }
    return 0;
}

/*
 * 判断出环的a字形角点用的弧形判断
 * 因为加上了丢线判断之后会无法判断出环的a字形角点 这个判断去除丢线条件
 * status:1左2右    num：A点的差值    start大 end小
 * */
int RoundaboutGet_A(track&track, int status, int num,int start,int end )
{

    int i = 0;
    switch(status)
    {
    //左边线
      case 1:
          //从下到上遍历
        for(i =start-2; i >end; i--)
        {
            //
            if(track.l_border[i].y<track.l_border[i+1].y&&(track.l_border[i+1].y-track.l_border[i].y>num)&&(track.l_border[i+1].y-track.l_border[i].y<COLSIMAGE*2/3))
            {
                return 1;
            }

        }

        break;

      case 2:
        for(i =start-2; i > end; i--)
        {


                /* 有弧线 */
                if(track.r_border[i].y>track.r_border[i+1].y&&(track.r_border[i].y-track.r_border[i+1].y>num)&&(track.r_border[i].y-track.r_border[i+1].y<COLSIMAGE*2/3))
                {
                    return 1;
                }
        }
        break;
    }

    return 0;
}



public:
//入环步骤
RingStep state;
//1:向上（从右开始）2：向下
int RoadUpSide_Mono(track&track,int X1, int X2,int down_max)
{
     //找上下边线
     UpdownSideGet(track);

     int i = 0, num = 0;

     for(i = X1; i < X2-1; i++)
     {
        //向上
         if(imageOut[0][i] >= imageOut[0][i+1]&&imageOut[0][i]!=0&&imageOut[0][i]<down_max)
             num++;
         else
             num = 0;
         if (num >= (X2-X1)*4/5)
             return 1;
     }
     //向下
     for(i = X1; i < X2-1; i++)
     {
         if(imageOut[0][i] <= imageOut[0][i+1]&&imageOut[0][i]!=0&&imageOut[0][i]<down_max)
             num++;
         else
             num = 0;
         if (num >= (X2-X1)*4/5)
             return 2;
     }
     return 0;
}
int RoadUpSide_Mono_out(track&track,int X1, int X2,int down_max)
{

    //找上下边线
    UpdownSideGet(track);

    int i = 0, num = 0,dec=0;

    // for (i=X1+1;i<X2-1;i++)
    // {
    //     if(track.l_border[i].y==0&&track.r_border[i].y==COLSIMAGE-1)
    //     {
    //         num++;
    //         dec=0;
    //         if(num==SQUARE*2)
    //         {
    //             return 1;
    //         }
    //     }else{
    //         dec++;
    //         if(dec==SQUARE/2)
    //         {
    //             dec=0;
    //             num=0;
    //         }
    //     }

    // }
    for(i = X1+1; i < X2-1; i++)
    {
    //向上
        if(imageOut[0][i]<down_max)
            num++;
        else
        {
            dec++;
            if(dec>=SQUARE)
            {
                num = 0;
                dec=0;
            }
        }
        if (num >= (X2-X1)*4/5)
            return 1;
    }
    // //向下
    // for(i = X1+1; i < X2-1; i++)
    // {
    //     if(imageOut[0][i] <= imageOut[0][i+1]&&imageOut[0][i]!=0&&imageOut[0][i]<down_max)
    //         num++;
    //     else
    //     {
    //         dec++;
    //         if(dec>=10)
    //         {
    //             num = 0;
    //             dec=0;
    //         }
    //     }
    //     if (num >= (X2-X1)*4/5)
    //         return 2;
    // }
    return 0;
}
//补直线
//1：左补线  2：右补线
void ImageAddingLine(track&track, int status, int startX, int startY, int endX, int endY)
{
    int i = 0;
    // 直线 x = ky + b
    float k = 0.0f, b = 0.0f;
    while(1)
        {
        if(status==1)//左补线
            {
                k = (float)((float)endX - (float)startX) / (float)((float)endY - (float)startY);
                b = (float)startX - (float)startY * k;

                for(i = startY; i < endY; i++)
                {
                track.l_border[i].y = (int)(k * i + b);
                }
                break;
            }

        if(status==2)//右补线
            {
                k = (float)((float)endX - (float)startX) / (float)((float)endY - (float)startY);
                b = (float)startX - (float)startY * k;

                for(i = startY; i < endY; i++)
                {
                track.r_border[i].y = (int)(k * i + b);
                }
                break;
            }

        }
}

//是否为圆环
int RoadIsRoundabout( track & track)
{
    int i = 0;
    int errL=0;
    int errR=0;
    int leftState = 0;
    int rightState = 0;
    int count = 0;
    int num = 0, py;
    int dec = 0;
    int lose_num=0;
    // 从车头往前 左边线是否单调
    for (i=END_H;i<START_H-2;i++)
    {
        if(track.l_border[i].y == 0)//因为track寻边线的时候就将找不到边线的位置分别置成0和COLSIMAGE-1
            continue;
        if(track.l_border[i].y >= track.l_border[i+1].y)    // i是Y坐标值  0 是图像左线X坐标
        {
            if(track.l_border[i].y<COLSIMAGE/2)
            {
                num++;
            }
            else
            {
                break;
            }
        }
        else
        {
            // num=0;
            continue;
        }
        if(i <= END_H+1)  //   清0
            num = 0;
    }       
    if(num >= mono_num)
    {
        num = 0;
        leftState = 1;   // 左单调标志
    }else{
        num=0;
    }

    errL = RoundaboutGetArc(track, 1, round_size, &py,ROWSIMAGE,5,ROWSIMAGE/2,0);//round_size圆弧的大小      圆弧的大小 用点数表示  （连续N个增  连续N个减）
    errR = RoundaboutGetArc(track, 2, round_size, &py,ROWSIMAGE,54,ROWSIMAGE/2,0);//1是左边线   2是右边线

    // 右边线是否单调
    for (i=END_H;i<START_H-2;i++)
    {
        if(track.r_border[i].y == COLSIMAGE-1)
            continue;
        if(track.r_border[i].y<= track.r_border[i+1].y)
        {
            if(track.r_border[i].y>COLSIMAGE/2)
            {
                num++;
            }
            else
            {
                break;
            }
        }
        else
        {
            // num=0;
            continue;
        }
            if(i <= END_H+1)
            num = 0;
    }
    if(num >= mono_num)
    {
        num = 0;
        rightState = 1;
    }else{
        num=0;
    }
    _right_cir=errR;
    _left_cir==errL;
    _left_mono=leftState;
    _right_mono=rightState;
    // 左边单调， 检测右侧是否是环岛
    if(leftState == 1 && rightState == 0 && errL == 0)
    {
        count = 0;

        if(RoundaboutGetArc(track, 2, round_size, &py,ROWSIMAGE,5,ROWSIMAGE/2,10))
        {
            ring_type=right_ring;
            state=RingStep::Prepareing;
            return 1;
        }
        else
        {
            return 0;
        }
    }
    /* 右边单调， 检测左侧是否是环岛 */
    if(rightState == 1 && leftState == 0&& errR == 0)
    {
        count = 0;
        if(RoundaboutGetArc(track, 1, round_size, &py,ROWSIMAGE,5,ROWSIMAGE/2,10))
        {
            ring_type=left_ring;
            state=RingStep::Prepareing;
            return 2;
        }
    }
    return 0;
}



 //化线
void RoundaboutProcess(track & track)
{
    int i = 0, err5 = 0,err6 = 0;
    int pointX = 0, pointY = 0, inc = 0, dec = 0;
    int flag= 0, Down_flag = 0;
    static int finderr = 0;
    static int finderr_l=0;
    static int err1 = 0;
    static int err2 = 0;
    static int up_flag=0;
    int finish_flag=0;
    int finish_lose;

      //奇数为入右圆环   偶数进入左圆环
    if(ring_type==right_ring)
    {
        switch(state)
        {
        case RingStep::Prepareing:
            // 检查弧线
            err1 = RoundaboutGetArc(track,2, 3, &pointY,ROWSIMAGE,0,ROWSIMAGE,0);

            // 有弧线 进行补线 连接弧线最右点 和 图像右下角
            if(err1)
            {
                pointX = track.r_border[pointY].y;
                ImageAddingLine(track,2, pointX, pointY, pre_add_r , ROWSIMAGE);
                finderr = 1;
            }
            else
            {
                if(finderr)
                    state = RingStep::Entering;//准备进入环岛
            }
            
            /* 发现左环岛 环岛出口处补线 */

        break;
        case RingStep::Entering:
            for(i=1;i<COLSIMAGE-1;i++)
            {
                track.l_border[i].y=  track.l_border[i].y+ent_add;
            }
            if(RoadUpSide_Mono(track,0, COLSIMAGE-1,ROWSIMAGE) == 1)//上线单调增进入下一步
                state = RingStep::Inside;
        break;
        case RingStep::Inside:
            err5 = RoundaboutGet_A(track, 1, 5,ROWSIMAGE-1,ROWSIMAGE/2);
            _test_A=err5;
            flag = RoadUpSide_Mono(track,0, COLSIMAGE-1,ROWSIMAGE/2);
            _test_UP=flag;
            //检查下线
            // for(i = COLSIMAGE-1; i > 0; i--)
            // {
            //     if(imageOut[1][i] ==ROWSIMAGE-1)
            //         inc++;
            //     else
            //         dec++;
            //     if( dec >= 20)
            //     {
            //         Down_flag = 1;
            //         break;
            //     }
            // }
            //不使用上线单调,因为想使用上线断掉之后再连起来做出环判断
            if(err5 &&(flag==2||flag==1))
            {
                state = RingStep::Exiting;
            }
        break;
        case RingStep::Exiting:
            ImageAddingLine(track, 1, exit_add_r, 0, exit_add_r, ROWSIMAGE-1);
            ImageAddingLine(track, 2, COLSIMAGE-1, 0, COLSIMAGE-1, ROWSIMAGE-1);
            flag = RoadUpSide_Mono(track,5, COLSIMAGE-5,ROWSIMAGE);

            if(flag==1)
            {
                up_flag=1;
            }
            _test_UP=up_flag;
            _test_A=flag;
            if(flag==0&&up_flag==1)
            {
                state = RingStep::Finish;
                preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                startTime=preTime;
                finderr = 0;
                err1 = 0;
                up_flag=0;
            }
        break;
        case RingStep::Finish:
            finish_lose=0;
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            ImageAddingLine(track, 1, exit_add_r, 0, exit_add_r, ROWSIMAGE-1);
            ImageAddingLine(track, 2, COLSIMAGE-1, 0, COLSIMAGE-1, ROWSIMAGE-1);
            if((startTime-preTime)>cir_delay_max)
            {
                state=RingStep::Wait;
                preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                startTime=preTime;

            }
            for(i=FINISH_SATRT;i<ROWSIMAGE;i++)
            {
                if(track.l_border[i].y==0)
                {
                    finish_lose++;
                }
            }
            if(finish_lose<FINISH_LOSE_NUM)
            {
                if((startTime-preTime)<cir_delay)
                {
                    break;
                }else{
                    state=RingStep::Wait;
                    preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                    startTime=preTime;
                }
            }

        break;
        case RingStep::Wait:
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            if((startTime-preTime)>cir_wait)
            {
                state=RingStep::None;
            }
        break;
        }
    }
    if(ring_type==left_ring)
    {
        switch(state)
        {
        case RingStep::Prepareing:
            // 检查弧线
            err2 = RoundaboutGetArc(track, 1, 3, &pointY,ROWSIMAGE,0,ROWSIMAGE,0);
            // 有弧线 进行补线 连接弧线最左点 和 图像左下角
            if(err2)
            {
                pointX = track.l_border[pointY].y;
                ImageAddingLine(track,1, pointX, pointY, pre_add_l , ROWSIMAGE);
                finderr_l = 1;
            }
            else
            {
                if(finderr_l)//判断需要加
                {state = RingStep::Entering;}//准备进入环岛
            }
        break;
            /* 发现右环岛 环岛出口处补线 */
        //上边线单调
        case RingStep::Entering:
            for(i=1;i<COLSIMAGE-1;i++)
                {
                    track.r_border[i].y= track.r_border[i].y-ent_add;
                }

            if(RoadUpSide_Mono(track,0,COLSIMAGE-1,ROWSIMAGE) == 2)//上线单调增进入下一步
                    {state = RingStep::Inside;}
        break;
        case RingStep::Inside:
            //出赛道识别的拐点     不用弧线判别 拐到能看见上边先的时候已经变成了A字突变点 使用拐点判别的方式
            err6 = RoundaboutGet_A(track, 2, 5,ROWSIMAGE-1,ROWSIMAGE/2);
            //上边线单调？
            flag = RoadUpSide_Mono(track,0,COLSIMAGE-1,ROWSIMAGE/2);
            _test_UP=flag;
            _test_A=err6;
            //检查下线
            // for(i = COLSIMAGE-1; i > 0; i--)
            // {
            //     if(imageOut[1][i] == ROWSIMAGE-1)
            //         inc++;
            //     else
            //         dec++;
            //     if( dec <= 20)
            //     {
            //         Down_flag = 1;
            //         break;
            //     }
            // }
    //                         if((flag==1||flag==2)&& err6&& Down_flag)
            if(err6&& (flag==2||flag==1))
            {

                state = RingStep::Exiting;
            }
        break;
        case RingStep::Exiting:
                //判断元素
                //补线
                ImageAddingLine(track, 2,exit_add_l,0,exit_add_l, ROWSIMAGE-1);
                ImageAddingLine(track, 1,0,0,0, ROWSIMAGE-1);
                //上边线
                flag = RoadUpSide_Mono(track,5,COLSIMAGE-5,ROWSIMAGE);

                if(flag==2)
                {
                    up_flag=1;
                }
                if(flag==0&&up_flag==1)
                {
                    state = RingStep::Finish;
                    preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                    startTime=preTime;
                    finderr_l = 0;
                    err2 = 0;
                    up_flag=0;
                }
        break;
        case RingStep::Finish:
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            finish_lose=0;
            ImageAddingLine(track, 2,exit_add_l,0,exit_add_l, ROWSIMAGE-1);
            ImageAddingLine(track, 1,0,0,0, ROWSIMAGE-1);
            if((startTime-preTime)>cir_delay_max)
            {
                state=RingStep::Wait;
                preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                startTime=preTime;

            }

            for(i=FINISH_SATRT;i<ROWSIMAGE;i++)
            {
                if(track.r_border[i].y==ROWSIMAGE-1)
                {
                    finish_lose++;
                }
            }
            if(finish_lose<FINISH_LOSE_NUM)
            {
                if((startTime-preTime)<cir_delay)
                {
                    break;
                }else{
                    state=RingStep::Wait;
                    preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                    startTime=preTime;
                }
            }

        break;
        case RingStep::Wait:
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            if((startTime-preTime)>cir_wait)
            {
                state=RingStep::None;
            }
        break;
        }
    }
    std::cout<<state<<std::endl;
}

void draw(Mat & binary_mat)
{
    if(state!=RingStep::None)
    {
        putText(binary_mat,"RingStep:"+to_string(state) , Point(COLSIMAGE/3,15),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    }
    
    putText(binary_mat, "circle_test", Point(COLSIMAGE - 100, ROWSIMAGE - 50),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    putText(binary_mat, "out_A"+to_string(_test_A),Point(COLSIMAGE -100,ROWSIMAGE-70),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    putText(binary_mat, "out_up"+to_string(_test_UP),Point(COLSIMAGE -100,ROWSIMAGE-90),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    putText(binary_mat, "right mono"+to_string(_right_mono),Point(COLSIMAGE -100,ROWSIMAGE-30),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    putText(binary_mat, "right cir"+to_string(_right_cir),Point(COLSIMAGE -100,ROWSIMAGE-10),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    putText(binary_mat, "left mono"+to_string(_left_mono),Point(COLSIMAGE -200,ROWSIMAGE-30),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    putText(binary_mat, "left cir"+to_string(_left_cir),Point(COLSIMAGE -200,ROWSIMAGE-10),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);

}


};